﻿using System;
using System.Collections.Generic;
using System.Collections;
using System.Drawing;

namespace MonoPhysics
{
    /// <summary>
    /// Moteur physique.
    /// </summary>
    public static class NarrowPhase
    {

        public static bool Rule(Edge e)
        {
            return ((e.ue1.And_objectGroup & e.ue2.And_objectGroup) != 0) &&
                (((((e.ue1.Xnor_objectGroup & 1) & (e.ue2.Xnor_objectGroup & 1)) == 1 && ((((e.ue1.Xnor_objectGroup & 2) & (e.ue2.Xnor_objectGroup & 2)) >> 1)) == 1)
                || (((e.ue1.Xnor_objectGroup & 1) & (e.ue2.Xnor_objectGroup & 1)) == 0 && ((((e.ue1.Xnor_objectGroup & 2) & (e.ue2.Xnor_objectGroup & 2)) >> 1)) == 0)))
                && (((e.ue1.Xnor_objectGroup & 4) & (e.ue2.Xnor_objectGroup & 4)) == 0);
        }

        public static bool Hascontacts(Edge e, bool conciderCentroid)
        {
            return Hascontacts(e, 0, conciderCentroid);
        }

        public static bool Hascontacts(Edge e, float delta, bool conciderCentroid)
        {
            List<SegmentD> seg = new List<SegmentD>();
            List<PointD> ptsFound = new List<PointD>();
            List<SegmentCouple> scps = new List<SegmentCouple>();
            List<SphereCollision> scs = new List<SphereCollision>();
            GeometryHelper.GetSphereTreeCollisions(scs, ((Polygon)e.ue1.Shape).Tree.Tree, ((Polygon)e.ue2.Shape).Tree.Tree);
            Queue<CircularLinkedListNode<PointD, RigidBody>> nodes = new Queue<CircularLinkedListNode<PointD, RigidBody>>();
            foreach (SphereCollision sc in scs)
            {
                foreach (SegmentD s in sc.c1.lyingSegments)
                {
                    foreach (SegmentD s2 in sc.c2.lyingSegments)
                    {
                        PointD inter;
                        PointD endInter;
                        SegmentCouple couple = new SegmentCouple(s, s2);
                        if (GeometryHelper.Get2SegmentsIntersection(s, s2, out inter, out endInter, 0.000000001f) != 0)
                        {
                            if (delta != 0)
                            {
                                float dx = s.Pt1.X - inter.X;
                                float dy = s.Pt1.Y - inter.Y;

                                if (dx * dx + dy * dy <= delta)
                                    continue;
                                dx = s.Pt2.X - inter.X;
                                dy = s.Pt2.Y - inter.Y;
                                if (dx * dx + dy * dy <= delta)
                                    continue;
                                dx = s2.Pt1.X - inter.X;
                                dy = s2.Pt1.Y - inter.Y;
                                if (dx * dx + dy * dy <= delta)
                                    continue;
                                dx = s2.Pt2.X - inter.X;
                                dy = s2.Pt2.Y - inter.Y;
                                if (dx * dx + dy * dy <= delta)
                                    continue;
                            }
                            return true;
                        }
                    }
                }
            }
            // il y a peut etre une penetration
            return /*(dontTryPtPoly) ? false :*/ GeometryHelper.PointDinPolygon((Polygon)(e.ue1.Shape), e.ue1.Pos, (conciderCentroid) ? (PointD)(e.ue2.Pos) : ((Polygon)e.ue2.Shape).Points.value + e.ue2.Pos, 100) || GeometryHelper.PointDinPolygon((Polygon)(e.ue2.Shape), e.ue2.Pos, (conciderCentroid) ? (PointD)e.ue1.Pos : ((Polygon)e.ue1.Shape).Points.value + e.ue1.Pos, 100);
        }
        /// <summary>
        /// Construit un couple de colision entre deux elements.
        /// </summary>
        /// <param name="u1">Premier element qui sert de reference.</param>
        /// <param name="u2">Second element.</param>
        /// <returns>Renvoie le couple de colision corespondant si il y a contact. Renvoie null si il n'y a ni contact ni penetration. Renvoie une structure vide si il y a penetration.</returns>
        public static bool GetContacts(MonoPhysics engine, Edge e)
        {
            bool colludedOnce = false;
            RigidBody u1 = e.ue1, u2 = e.ue2;
            bool bothMoveable = e.ue1.M != 0 && e.ue2.M != 0;
            float u1Mass = 0;
            float u2Mass = 0;
            if (bothMoveable)
            {
                u2Mass = u2.M;
                u1Mass = u1.M;
                if (!u1.AcceptFusionWith(u2, null))
                {
                    u2.M = 0;
                }
                else
                {
                    u1.M = 0;
                }
            }
            Collision c = new Collision();
            e.hasPenatrate = false;
            e.collisions = new List<Collision>();
            List<SegmentD> seg = new List<SegmentD>();
            List<PointD> ptsFound = new List<PointD>();
            List<SegmentCouple> scps = new List<SegmentCouple>();
            List<SphereCollision> scs = new List<SphereCollision>();
            GeometryHelper.GetSphereTreeCollisions(scs, ((Polygon)u1.Shape).Tree.Tree, ((Polygon)u2.Shape).Tree.Tree);
            Queue<CircularLinkedListNode<PointD, RigidBody>> nodes = new Queue<CircularLinkedListNode<PointD, RigidBody>>();

            foreach (SphereCollision sc in scs)
            {
                foreach (SegmentD s in sc.c1.lyingSegments)
                {
                    foreach (SegmentD s2 in sc.c2.lyingSegments)
                    {
                        PointD inter;
                        PointD endInter;
                        SegmentCouple couple = new SegmentCouple(s, s2);
                        bool cont = false;
                        foreach (SegmentCouple scpl in scps)
                        {
                            if (scpl.equals(couple))
                            {
                                cont = true;
                                break;
                            }
                        }
                        if (cont)
                            continue;
                        scps.Add(couple);
                        int res = GeometryHelper.Get2SegmentsIntersection(s, s2, out inter, out endInter, 0.000000001f);
                        if (res != 0)
                        {
                            c.collisionPoint = inter;
                            /* foreach (PointD p in ptsFound)
                             {
                                 if ((p - inter).isZero(0.1f))
                                 {
                                     cont = true;
                                     break;
                                 }
                             }
                             if (cont)
                                 continue;
                             //*/
                            ptsFound.Add(inter);
                            colludedOnce = true;
                            if (sc.c1.Pere.M == 0)
                            {
                                //Vector v = new Vector(pt, prev);
                                //v = v.direction();
                                //Vector edge = new Vector(prev, curr);
                                //Vector cprdct1 = (edge.cross(v));
                                c.arreteColuded = new Vector(s.Pt1, s.Pt2);
                                sc.c2.Pere.contactTgts.Add(c.arreteColuded);
                                c.collisionNormal = c.arreteColuded.direction();
                                c.collisionNormal = new Vector(-c.collisionNormal.Y, c.collisionNormal.X, 0);
                                c.collisionType = Collision.CollisionType.PointArrete;
                                //c.arreteGauche = new Vector(curr, suiv);
                                //c.arreteDroite = new Vector(prev, prevprev);
                                Vector v2CollisionPoint = new Vector(new Vector(c.collisionPoint) - sc.c2.Pere.Pos);
                                Vector v1CollisionPoint = new Vector(new Vector(c.collisionPoint) - sc.c1.Pere.Pos);
                                c.relativeVelocity = ((sc.c2.Pere.PhysicStats.V + (sc.c2.Pere.PhysicStats.AngularVitess * new Vector(0, 0, 1)).cross(v2CollisionPoint)) - (sc.c1.Pere.PhysicStats.V + (sc.c1.Pere.PhysicStats.AngularVitess * new Vector(0, 0, 1)).cross(v1CollisionPoint)));
                                sc.c2.Pere.CollisionPoints.Add(new PointD(v2CollisionPoint.X, v2CollisionPoint.Y));
                                if (e.type != Edge.EdgeType.PersistantContact && e.type != Edge.EdgeType.Contact)
                                    e.collisions.Add(c);

                                // forces de contact
                                else
                                {
                                    Vector rvel = c.relativeVelocity;// (sc.c2.Pere.PhysicStats.V - (new Vector(sc.c1.Pere.PhysicStats.V.X, sc.c1.Pere.PhysicStats.V.Y, 0)));//.direction();
                                    Vector tangeant = (rvel.dot(c.arreteColuded) * c.arreteColuded).reflexion().direction(); //(rvel - ((rvel.dot(c.collisionNormal) * c.collisionNormal))).reflexion();
                                    Force contactForce = new Force();
                                    contactForce.force = (sc.c2.Pere.M * (-rvel.dot(c.collisionNormal))) * c.collisionNormal;//(sc.c2.Pere.M * ((new Vector(0, -9.81f, 0)).dot(c.collisionNormal)) * c.collisionNormal);
                                    contactForce.location = v2CollisionPoint;
                                    Force frictionForce = new Force();
                                    frictionForce.force = (contactForce.force.Magnitude * 0.8f) * tangeant;
                                    frictionForce.location = v2CollisionPoint;
                                    sc.c2.Pere.PhysicStats.ExtraForces.Add(frictionForce);
                                    sc.c2.Pere.PhysicStats.ExtraForces.Add(contactForce);
                                }
                            }
                            else
                            {
                                //Vector v = new Vector(pt, pre1v);
                                //v = v.direction();
                                //Vector edge = new Vector(prev, curr);
                                //Vector cprdct1 = (edge.cross(v));
                                c.arreteColuded = new Vector(s2.Pt1, s2.Pt2);
                                sc.c1.Pere.contactTgts.Add(c.arreteColuded);
                                c.collisionNormal = c.arreteColuded.direction();
                                c.collisionNormal = new Vector(-c.collisionNormal.Y, c.collisionNormal.X, 0);
                                c.collisionType = Collision.CollisionType.PointArrete;
                                //c.arreteGauche = new Vector(curr, suiv);
                                //c.arreteDroite = new Vector(prev, prevprev);
                                //c.arreteColuded = new Vector(curr, prev);
                                Vector v2CollisionPoint = new Vector(new Vector(c.collisionPoint) - sc.c2.Pere.Pos);
                                Vector v1CollisionPoint = new Vector(new Vector(c.collisionPoint) - sc.c1.Pere.Pos);
                                c.relativeVelocity = (sc.c1.Pere.PhysicStats.V + (sc.c1.Pere.PhysicStats.AngularVitess * new Vector(0, 0, 1)).cross(v1CollisionPoint)) - (sc.c2.Pere.PhysicStats.V + (sc.c2.Pere.PhysicStats.AngularVitess * new Vector(0, 0, 1)).cross(v2CollisionPoint));
                                sc.c1.Pere.CollisionPoints.Add(new PointD(v1CollisionPoint.X, v1CollisionPoint.Y));
                                if (e.type != Edge.EdgeType.PersistantContact && e.type != Edge.EdgeType.Contact)
                                    e.collisions.Add(c);

                                // forces de contact

                                else
                                {
                                    Vector rvel = c.relativeVelocity;//sc.c1.Pere.PhysicStats.V - (new Vector(sc.c2.Pere.PhysicStats.V.X, sc.c2.Pere.PhysicStats.V.Y, 0));//sc.c1.Pere.PhysicStats.V+ sc.c2.Pere.PhysicStats.V;//.direction();
                                    Vector tangeant = (rvel.dot(c.arreteColuded) * c.arreteColuded).reflexion().direction(); ;//(rvel - ((rvel.dot(c.collisionNormal) * c.collisionNormal))).reflexion();
                                    Force contactForce = new Force();
                                    contactForce.force = (sc.c1.Pere.M * (-rvel.dot(c.collisionNormal))) * c.collisionNormal; //(sc.c1.Pere.M * ((new Vector(0, -9.81f, 0)).dot(c.collisionNormal)) * c.collisionNormal);
                                    contactForce.location = v1CollisionPoint;
                                    Force frictionForce = new Force();
                                    frictionForce.force = (contactForce.force.Magnitude * 0.8f) * tangeant;
                                    frictionForce.location = v1CollisionPoint;
                                    sc.c1.Pere.PhysicStats.ExtraForces.Add(frictionForce);
                                    sc.c1.Pere.PhysicStats.ExtraForces.Add(contactForce);
                                }
                            }

                            #region Construction flexible
                            // On verifie d'abord si tout ca va servir a quelquechose...
                            if (bothMoveable)
                            {
                                if (!e.ue1.AcceptFusionWith(e.ue2, e) || !e.ue2.AcceptFusionWith(e.ue1, e))
                                {
                                    e.PenetraDepth.Clear();
                                    e.penNormal = new Vector();
                                    return true;
                                }
                            }
                            // seulement ensuite on calcule!
                            SegmentD seg1 = s;
                            SegmentD seg2 = s2;// Operation d'intersection: on bouje les pointeurs! Et on met dans l'edge les points qui ont ete modifies pour pouvoir les reinitialiser au frame suivent


                            //if (u1.M != 0) // u1 est l'objet fixe

                            if (u1.M == 0)
                            {
                                if (u2.M == 0)
                                {
                                    if (!u1.PhysicStats.V.isNull())
                                        MathGeometryHelper.swap<RigidBody>(ref u1, ref u2); ;
                                }
                            }
                            else
                                MathGeometryHelper.swap<RigidBody>(ref u1, ref u2);


                            if (s.LinkPt1.FondamentalColor != ((Polygon)u1.Shape).Color)
                                MathGeometryHelper.swap<SegmentD>(ref seg1, ref seg2);

                            float dist = MathGeometryHelper.DistPtDrt(seg1.Pt1, seg1.Pt2, seg2.Pt2);
                            float dist2 = MathGeometryHelper.DistPtDrt(seg1.Pt1, seg1.Pt2, seg2.Pt1);
                            //if (Math.Abs(dist) <=1 || Math.Abs(dist2)<=1)
                            //    continue;
                            #region Recherche des bornes d'insertion pour le lien noir:
                            CircularLinkedListNode<PointD, RigidBody> borneGauche = seg1.LinkPt2;
                            CircularLinkedListNode<PointD, RigidBody> borneDroite = seg1.LinkPt1;
                            int timerStop;
                            if (seg1.Pt1.X != seg1.Pt2.X)
                            {// on peut les comparer selon X
                                CircularLinkedListNode<PointD, RigidBody> node = borneGauche;
                                timerStop = 0;
                                do
                                {
                                    timerStop++;
                                    foreach (CircularLinkedListColoredNode<PointD, RigidBody> cnode in node.nodes)
                                    {
                                        if (cnode.couleur == ((Polygon)u1.Shape).Color)
                                        {
                                            borneGauche = node;
                                            node = cnode.parent;
                                            break;
                                        }
                                    }
                                } while (Math.Abs(inter.X - (borneGauche.value.X + borneGauche.innerParent.Pos.X)) > Math.Abs((node.value.X + node.innerParent.Pos.X) - (borneGauche.value.X + borneGauche.innerParent.Pos.X)) && timerStop < short.MaxValue);
                                // <- Ok
                                node = borneDroite;
                                timerStop = 0;
                                do
                                {
                                    timerStop++;
                                    foreach (CircularLinkedListColoredNode<PointD, RigidBody> cnode in node.nodes)
                                    {
                                        if (cnode.couleur == ((Polygon)u1.Shape).Color)
                                        {
                                            borneDroite = node;
                                            node = cnode.fils;
                                            break;
                                        }
                                    }
                                } while (Math.Abs(inter.X - (borneDroite.value.X + borneDroite.innerParent.Pos.X)) > Math.Abs((node.value.X + node.innerParent.Pos.X) - (borneDroite.value.X + borneDroite.innerParent.Pos.X)) && timerStop < short.MaxValue);
                                // <- OK
                            }
                            #region // on compare selon y
                            else
                            {
                                CircularLinkedListNode<PointD, RigidBody> node = borneGauche;
                                timerStop = 0;
                                do
                                {
                                    timerStop++;
                                    foreach (CircularLinkedListColoredNode<PointD, RigidBody> cnode in node.nodes)
                                    {
                                        if (cnode.couleur == ((Polygon)u1.Shape).Color)
                                        {
                                            borneGauche = node;
                                            node = cnode.parent;
                                            break;
                                        }
                                    }
                                } while (Math.Abs(inter.Y - (borneGauche.value.Y + borneGauche.innerParent.Pos.Y)) > Math.Abs((node.value.Y + node.innerParent.Pos.Y) - (borneGauche.value.Y + borneGauche.innerParent.Pos.Y)) && timerStop < short.MaxValue);
                                // <- Ok ?
                                node = borneDroite;
                                timerStop = 0;
                                do
                                {
                                    timerStop++;
                                    foreach (CircularLinkedListColoredNode<PointD, RigidBody> cnode in node.nodes)
                                    {
                                        if (cnode.couleur == ((Polygon)u1.Shape).Color)
                                        {
                                            borneDroite = node;
                                            node = cnode.fils;
                                            break;
                                        }
                                    }
                                } while (Math.Abs(inter.Y - (borneDroite.value.Y + borneDroite.innerParent.Pos.Y)) > Math.Abs((node.value.Y + node.innerParent.Pos.Y) - (borneDroite.value.Y + borneDroite.innerParent.Pos.Y)) && timerStop < short.MaxValue);
                                // <- OK ?
                            }
                            #endregion
                            #endregion

                            #region Recherche des bornes d'insertion pour le lien bleu:
                            CircularLinkedListNode<PointD, RigidBody> borneBleueGauche = seg2.LinkPt1;
                            CircularLinkedListNode<PointD, RigidBody> borneBleueDroite = seg2.LinkPt2;
                            if (seg2.Pt1.X != seg2.Pt2.X)
                            {// on peut les comparer selon X
                                timerStop = 0;
                                CircularLinkedListNode<PointD, RigidBody> node = borneBleueGauche;
                                do
                                {
                                    timerStop++;
                                    foreach (CircularLinkedListColoredNode<PointD, RigidBody> cnode in node.nodes)
                                    {
                                        if (cnode.couleur == ((Polygon)u2.Shape).Color)
                                        {
                                            borneBleueGauche = node;
                                            node = cnode.fils;
                                            break;
                                        }
                                    }
                                } while (Math.Abs(inter.X - (borneBleueGauche.value.X + borneBleueGauche.innerParent.Pos.X)) > Math.Abs((node.value.X + node.innerParent.Pos.X) - (borneBleueGauche.value.X + borneBleueGauche.innerParent.Pos.X)) && timerStop < short.MaxValue);
                                //<- OK
                                node = borneBleueDroite;
                                timerStop = 0;
                                do
                                {
                                    timerStop++;
                                    foreach (CircularLinkedListColoredNode<PointD, RigidBody> cnode in node.nodes)
                                    {
                                        if (cnode.couleur == ((Polygon)u2.Shape).Color)
                                        {
                                            borneBleueDroite = node;
                                            node = cnode.parent;
                                            break;
                                        }
                                    }
                                } while (Math.Abs(inter.X - (borneBleueDroite.value.X + borneBleueDroite.innerParent.Pos.X)) > Math.Abs((node.value.X + node.innerParent.Pos.X) - (borneBleueDroite.value.X + borneBleueDroite.innerParent.Pos.X)) && timerStop < short.MaxValue);
                                //<- OK
                            }
                            #region // on compare selon y
                            else
                            {
                                CircularLinkedListNode<PointD, RigidBody> node = borneBleueGauche;
                                timerStop = 0;
                                do
                                {
                                    timerStop++;
                                    foreach (CircularLinkedListColoredNode<PointD, RigidBody> cnode in node.nodes)
                                    {
                                        if (cnode.couleur == ((Polygon)u2.Shape).Color)
                                        {
                                            borneBleueGauche = node;
                                            node = cnode.fils;
                                            break;
                                        }
                                    }
                                } while (Math.Abs(inter.Y - (borneBleueGauche.value.Y + borneBleueGauche.innerParent.Pos.Y)) > Math.Abs((node.value.Y + node.innerParent.Pos.Y) - (borneBleueGauche.value.Y + borneBleueGauche.innerParent.Pos.Y)) && timerStop < short.MaxValue);
                                // <- OK?
                                node = borneBleueDroite;
                                timerStop = 0;
                                do
                                {
                                    timerStop++;
                                    foreach (CircularLinkedListColoredNode<PointD, RigidBody> cnode in node.nodes)
                                    {
                                        if (cnode.couleur == ((Polygon)u2.Shape).Color)
                                        {
                                            borneBleueDroite = node;
                                            node = cnode.parent;
                                            break;
                                        }
                                    }
                                } while (Math.Abs(inter.Y - (borneBleueDroite.value.Y + borneBleueDroite.innerParent.Pos.Y)) > Math.Abs((node.value.Y + node.innerParent.Pos.Y) - (borneBleueDroite.value.Y + borneBleueDroite.innerParent.Pos.Y)) && timerStop < short.MaxValue);
                                //<- OK?
                            }
                            #endregion
                            #endregion

                            // ajout d'un node: (objet fixe = rouge, mobile = bleu) Le noeud caracteristique a pour couleur le bleu. 
                            CircularLinkedListNode<PointD, RigidBody> newNode = new CircularLinkedListNode<PointD, RigidBody>();
                            newNode.value = inter - u2.Pos;
                            newNode.innerParent = u2;

                            #region Lien bleu <- OK
                            CircularLinkedListColoredNode<PointD, RigidBody> blueLink = new CircularLinkedListColoredNode<PointD, RigidBody>();
                            blueLink.fils = borneBleueDroite;
                            blueLink.parent = borneBleueGauche;
                            CircularLinkedListColoredNode<PointD, RigidBody> nodecherche = borneBleueDroite.nodes[0];
                            foreach (CircularLinkedListColoredNode<PointD, RigidBody> cnode in borneBleueDroite.nodes)
                            {
                                if (((Polygon)u2.Shape).Color == cnode.couleur)
                                {
                                    nodecherche = cnode;
                                    break;
                                }
                            }
                            nodecherche.parent = newNode;

                            nodecherche = borneBleueGauche.nodes[0];
                            foreach (CircularLinkedListColoredNode<PointD, RigidBody> cnode in borneBleueGauche.nodes)
                            {
                                if (((Polygon)u2.Shape).Color == cnode.couleur)
                                {
                                    nodecherche = cnode;
                                    break;
                                }
                            }
                            nodecherche.fils = newNode;
                            blueLink.couleur = ((Polygon)u2.Shape).Color;

                            #endregion
                            #region Lien noir <- OK
                            CircularLinkedListColoredNode<PointD, RigidBody> redLink = new CircularLinkedListColoredNode<PointD, RigidBody>();
                            redLink.fils = borneGauche;
                            redLink.parent = borneDroite;
                            nodecherche = borneGauche.nodes[0];
                            foreach (CircularLinkedListColoredNode<PointD, RigidBody> cnode in borneGauche.nodes)
                            {
                                if (((Polygon)u1.Shape).Color == cnode.couleur)
                                {
                                    nodecherche = cnode;
                                    break;
                                }
                            }
                            nodecherche.parent = newNode;

                            nodecherche = borneDroite.nodes[0];
                            foreach (CircularLinkedListColoredNode<PointD, RigidBody> cnode in borneDroite.nodes)
                            {
                                if (((Polygon)u1.Shape).Color == cnode.couleur)
                                {
                                    nodecherche = cnode;
                                    break;
                                }
                            }
                            nodecherche.fils = newNode;
                            redLink.couleur = ((Polygon)u1.Shape).Color;
                            #endregion

                            newNode.nodes.Add(blueLink);
                            newNode.nodes.Add(redLink);
                            if (dist < 0) // gauche
                            {
                                newNode.pointType = CircularLinkedListNode<PointD, RigidBody>.NodeType.PointSortant;
                            }
                            else
                            {
                                newNode.pointType = CircularLinkedListNode<PointD, RigidBody>.NodeType.PointEntrant;
                            }
                            // Quels noeuds seront reinitialises?
                            //if (borneBleueGauche.pointType == CircularLinkedListNode<PointD, RigidBody>.NodeType.pointNeutre)
                            e.NodesToReinit.Add(borneBleueGauche);
                            //if (borneDroite.pointType == CircularLinkedListNode<PointD, RigidBody>.NodeType.pointNeutre)
                            //{
                            //((Polygon)u1.Shape).SetPointsHead(borneDroite);
                            e.NodesToReinit.Add(borneDroite);
                            //}
                            //if (borneBleueDroite.pointType == CircularLinkedListNode<PointD, RigidBody>.NodeType.pointNeutre)
                            e.NodesToReinit.Add(borneBleueDroite);
                            //if (borneGauche.pointType == CircularLinkedListNode<PointD, RigidBody>.NodeType.pointNeutre)
                            //{
                            //((Polygon)u1.Shape).SetPointsHead(borneGauche);
                            e.NodesToReinit.Add(borneGauche);

                            //fusionSafeNode = newNode;

                            //}
                            #endregion
                            //if (newNode.pointType == CircularLinkedListNode<PointD, RigidBody>.NodeType.PointEntrant)
                            nodes.Enqueue(newNode);
                        }
                    }
                }
            }

            #region fusion
            if (bothMoveable && e.collisions.Count >= 2)
            { // Operation de fusion
                CircularLinkedListNode<PointD, RigidBody> parc = (((Polygon)e.ue1.Shape).Points.value.X + e.ue1.Pos.X < ((Polygon)e.ue2.Shape).Points.value.X + e.ue2.Pos.X) ? ((Polygon)e.ue1.Shape).Points : ((Polygon)e.ue2.Shape).Points;//fusionSafeNode;
                CircularLinkedListNode<PointD, RigidBody> head = parc;
                int otherColor = ((Polygon)u1.Shape).Color;
                List<PointD> pts = new List<PointD>();
                int loopProtection = 1000;
                do
                {
                    pts.Add((parc.value + (parc.innerParent.Pos - u1.Pos)));

                    if (parc.pointType == CircularLinkedListNode<PointD, RigidBody>.NodeType.PointEntrant && parc.nodes[1].couleur == otherColor)
                        parc = parc.nodes[1].fils; // on bifurque sur le lien noir
                    else
                        parc = parc.nodes[0].fils;
                    loopProtection--;
                }
                while (parc != head && loopProtection > 0);
                u1.M = u1Mass + u2.M;
                u1.I += u2.I;
                u1.ObjectFusion(pts, u2.PhysicStats.V, u2.PhysicStats.AngularVitess);
                u1.FusionedWith(u2);
                u2.DestroyBecauseFusionWith(u1);
                engine.RemoveRigidBody(u2);
                return true;

            }
            #endregion

            if (colludedOnce)
            {
                if (u1.M != 0) // u1 est l'objet fixe
                    MathGeometryHelper.swap<RigidBody>(ref u1, ref u2);
                while (nodes.Count != 0)
                {
                    CircularLinkedListNode<PointD, RigidBody> newNode = nodes.Dequeue();
                    Vector penetrationDepth;
                    if (newNode.pointType == CircularLinkedListNode<PointD, RigidBody>.NodeType.PointEntrant)
                    {
                        float longueurenfonc = 0;
                        PointD pt1 = newNode.value + u2.Pos;
                        PointD pt2;
                        int iter = 0;
                        while (newNode.pointType != CircularLinkedListNode<PointD, RigidBody>.NodeType.PointSortant && iter < 100)
                        {
                            iter++;
                            longueurenfonc += PointD.GetDist(newNode.value, newNode.nodes[0].fils.value);
                            newNode = newNode.nodes[0].fils;
                        }
                        pt2 = newNode.value + u2.Pos;
                        if (iter == 100)
                        {
                            //Console.WriteLine();
                            continue;
                        }
                        penetrationDepth = new Vector(pt2, pt1).direction();
                        penetrationDepth = new Vector(-penetrationDepth.Y, penetrationDepth.X, 0);
                        /*longueurnorm = PointD.GetDist(pt2, pt1);
                        float forceVal = 8 * u2.M * Math.Abs(longueurnorm - longueurenfonc);
                        Vector direction = new Vector(pt1 - u2.Pos);
                        direction = u2.PhysicStats.V - direction.cross(new Vector(0, 0, u2.PhysicStats.AngularVitess));
                        direction = direction.direction().reflexion();//*/
                        //new Vector(newNode.value + u2.Pos, pt1);
                        //direction = new Vector(-direction.Y, direction.X, 0).direction();
                        //direction.X -= u2.PhysicStats.V.X;
                        //u2.PhysicStats.ExtraForces.Add(new Force(direction * forceVal, new Vector(pt1 - u2.Pos)));


                        #region Calcul de la profondeur de penetration:

                        iter = 0;
                        float lowerDepth = 0;
                        float upperDepth = 0;
                        float dist = Math.Abs(MathGeometryHelper.DistPtDrt(pt2, pt1, newNode.value + newNode.innerParent.Pos));
                        CircularLinkedListNode<PointD, RigidBody> originalNewNode = newNode;
                        ////Console.WriteLine(originalNewNode);
                        upperDepth = Math.Max(dist, upperDepth); // ignorera les valeurs negatives! (et donc les points a gauche
                        /*CircularLinkedListColoredNode<PointD, RigidBody> node1 = null;
                        int x = 0;
                        foreach (CircularLinkedListColoredNode<PointD, RigidBody> cnode in newNode.nodes)
                        {
                            if (cnode.couleur == ((Polygon)u1.Shape).Color)
                            {
                                node1 = cnode;
                                break;
                            }
                            x++;
                            if (x == 2)
                                //Console.WriteLine();
                        }*/
                        newNode = /*node1.fils;*/newNode.nodes[1].fils;
                        while (newNode.pointType != CircularLinkedListNode<PointD, RigidBody>.NodeType.PointEntrant && iter < 100)
                        {
                            iter++;
                            dist = -MathGeometryHelper.DistPtDrt(pt2, pt1, newNode.value + newNode.innerParent.Pos);
                            upperDepth = Math.Max(dist, upperDepth); // ignorera les valeurs negatives! (et donc les points a gauche
                            newNode = newNode.nodes[0].fils;
                        }
                        if (iter == 100)
                        {
                            //Console.WriteLine();
                            continue;
                        }
                        iter = 0;


                        while (newNode.pointType != CircularLinkedListNode<PointD, RigidBody>.NodeType.PointSortant && iter < 100)
                        {
                            iter++;
                            dist = Math.Abs(MathGeometryHelper.DistPtDrt(pt1, pt2, newNode.value + newNode.innerParent.Pos));
                            lowerDepth = Math.Max(dist, upperDepth); // ignorera les valeurs negatives! (et donc les points a gauche
                            newNode = newNode.nodes[0].fils;
                        }
                        if (iter == 100)
                        {
                            //Console.WriteLine();
                            continue;
                        }
                        penetrationDepth *= upperDepth + lowerDepth;
                        //if (upperDepth + lowerDepth > 200)
                            //Console.WriteLine();
                        e.PenetraDepth.Add(penetrationDepth);
                        //u2.PhysicStats.ExtraForces.Add(new Force(penetrationDepth * 100, new Vector(pt1 - u2.Pos)));
                        #endregion
                    }
                    else
                    {
                        float longueurenfonc = 0;
                        float longueurnorm = 0;
                        PointD pt1 = newNode.value + u2.Pos;
                        int iter = 0;
                        while (newNode.pointType != CircularLinkedListNode<PointD, RigidBody>.NodeType.PointEntrant && iter < 100)
                        {
                            iter++;
                            longueurenfonc += PointD.GetDist(newNode.value, newNode.nodes[0].parent.value);
                            newNode = newNode.nodes[0].parent;
                        }
                        if (iter == 100)
                        {
                            //Console.WriteLine();
                            continue;
                        }
                        longueurnorm = PointD.GetDist(newNode.value + u2.Pos, pt1);
                        float forceVal = 8 * u2.M * Math.Abs(longueurnorm - longueurenfonc);
                        Vector direction = new Vector(pt1 - u2.Pos);
                        direction = direction.cross(new Vector(0, 0, u2.PhysicStats.AngularVitess)).reflexion();
                        //if (direction.dot(new Vector(u1.Pos.X - pt1.X, u1.Pos.Y - pt1.Y, 0)) >= 0)
                        //    direction = u2.PhysicStats.V;
                        //else
                        direction += u2.PhysicStats.V;
                        direction = direction.direction().reflexion();
                        //new Vector(newNode.value + u2.Pos, pt1);
                        //direction = new Vector(-direction.Y, direction.X, 0).direction();
                        //direction.X -= u2.PhysicStats.V.X;
                        //u2.PhysicStats.ExtraForces.Add(new Force(direction * forceVal, new Vector(pt1 - u2.Pos)));
                    }
                }
            }
            if (bothMoveable)
            {
                u2.M = u2Mass;
                u1.M = u1Mass;
            }
            foreach (CircularLinkedListNode<PointD, RigidBody> n in e.NodesToReinit)
            {
                n.nodes[0].fils = n.nodes[1].fils;    // on repointe sur les nodes initiaux (pointeurs noirs)
                n.nodes[0].parent = n.nodes[1].parent;
            }
            e.NodesToReinit.Clear();
            ////Console.WriteLine("------------------------------------- sp: " + ((float)(t2 - t1)).ToString() + " // " + ((float)(SdlDotNet.Core.Timer.TicksElapsed - t2)).ToString() + "  |||   " + t3.ToString());
            return colludedOnce;
        }

    }
}
